#ifndef __CONTROL_H__
#define __CONTROL_H__

// #include <ros/ros.h>
// #include <geometry_msgs/Twist.h>
// #include <yolo_pkg/Yolo.h>
// #include <drone_pkg/SendString.h>
// #include <tf2_ros/transform_listener.h>
// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
// #include <tf/transform_datatypes.h>
#include <math.h>


class Control {
public:
    /*定义常量*/
    const double MAX_LINEAR_SPEED;                     // 最大线速度，单位 cm/s
    const double MAX_ANGULAR_SPEED;                    // 最大角速度，单位 度/s

    const double KP_LINEAR_SPEED_POINT;                // 坐标模式线速度的比例常数（KP），影响线速度的调整
    const double KP_LINEAR_SPEED_DETECT;               // 识别模式下线速度的比例常数（KP）
    const double KP_ANGULAR_SPEED;                     // 角速度环的比例常数（KP），通常用来调整角速度的反馈控制

    const double MIN_ERROR_XY_POINT;                   // 坐标模式下，XY方向允许的最小误差，单位 cm
    const double MIN_ERROR_XY_DETECT;                  // 识别模式下，XY方向允许的最小误差，单位 像素
    const double MIN_ERROR_YAW;                        // YAW角度的最小误差，单位 度

    /*目标变量*/
    double targetPointX;                               // 目标坐标X，单位 cm
    double targetPointY;                               // 目标坐标Y，单位 cm
    double targetDetectX;                              // 识别模式下目标的X坐标，单位 像素
    double targetDetectY;                              // 识别模式下目标的Y坐标，单位 像素
    double targetYaw;                                  // 目标角度YAW，单位度
    int targetNum;                                     // 识别到的目标数字（例如识别到的数字标识）

    /*中间变量*/
    double distanceXY;                                 // 坐标模式下目标点与当前位置的距离，单位 cm
    double errorPointX;                                // 坐标模式下X方向的偏差，单位 cm
    double errorPointY;                                // 坐标模式下Y方向的偏差，单位 cm
    double errorDetectX;                               // 识别模式下X方向的偏差，单位 像素
    double errorDetectY;                               // 识别模式下Y方向的偏差，单位 像素
    double actualPositionX;                            // 实际位置的X坐标，单位 cm
    double actualPositionY;                            // 实际位置的Y坐标，单位 cm
    double actualPositionYaw;                          // 实际位置的YAW角度，单位 度
    double detectX;                                    // 识别到目标的中心X坐标
    double detectY;                                    // 识别到目标的中心Y坐标
    double errorYaw;                                   // YAW方向的误差值，单位 度
    double cosTheta;                                   // 目标坐标角度的余弦值，用于计算目标方向
    double sinTheta;                                   // 目标坐标角度的正弦值，用于计算目标方向
    double startTime;                                  // 任务启动时间，单位秒
    double presentTime;                                // 当前时间，单位秒
    double deltaTime;                                  // 时间间隔，单位秒，用于计算时间差

    /*结果变量*/
    double resultLinearSpeedXY;                        // 计算得到的XY合成线速度
    double resultLinearSpeedX;                         // 计算得到的X方向速度
    double resultLinearSpeedY;                         // 计算得到的Y方向速度
    double resultAngularSpeedYaw;                      // 计算得到的Yaw方向角速度

    /*状态变量*/
    int flagMovePoint;                                 // 坐标飞行标志，表示是否处于坐标飞行模式
    int flagDetect;                                    // 目标点的标志位，表示是否检测到目标
    int mode;                                          // 当前的飞行模式（例如，坐标飞行模式、识别模式等）
    int detectNum;                                     // 识别到的目标数字（-1表示未识别到目标）
    int flagLaser;                                     // 激光灯状态，0为关闭，1为开启

    /*ROS相关*/
    // ros::NodeHandle nh;                                // ROS节点句柄，用于管理节点和通信
    // ros::Publisher pubSpeed;                           // 用于发布速度信息的ROS发布者
    // ros::Publisher pubString;                          // 用于发布字符串信息的ROS发布者
    // ros::Subscriber subNum;                            // 订阅器，用于接收数字识别信息
    // tf2_ros::Buffer tfBuffer;                          // 用于存储TF坐标变换信息
    // tf2_ros::TransformListener tfListener;             // 用于监听坐标变换信息

    /*相关函数*/     
    Control(double MaxLinerSpeed, double MaxAngularSpeed, double KPLP, double KPLD, double KPA, double MinXYP, double MinXYD, double MinYaw);
}

#endif/*__CONTROL_H__*/
